#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include "priority_queue.h"

#define LEFT_CLD(i) ((i<<1)+1)
#define RIGHT_CLD(i) ((i+1)<<1)
#define PARENT(i) ((i-1)>>1)

static void
swap(void **items, int i, int j)
{
	void *temp = items[i];
	items[i] = items[j];
	items[j] = temp;
}

static void 
min_heapify(void **items, comparison_func *compare, int heap_size, int i)
{
	int min = i;
	int left = LEFT_CLD(i);
	int right = RIGHT_CLD(i);
	if(left<heap_size && compare(items[min], items[left])>0)
		min = left;
	if(right<heap_size && compare(items[min], items[right])>0)
		min = right;
	if(min != i) {
		swap(items, i, min);
		min_heapify(items, compare, heap_size, min);
	}
}

struct Priority_Queue *
create(int size, comparison_func *comp)
{
	struct Priority_Queue *pqueue;
	if((pqueue=malloc(sizeof(struct Priority_Queue))) == NULL) {
		fprintf(stderr, "malloc error in: create\n");
		exit(1);
	}
	pqueue->compare = comp;
	pqueue->count = 0;
	pqueue->size = size;
	if((pqueue->items=malloc(sizeof(void *)*size)) == NULL) {
		fprintf(stderr, "malloc error in: create\n");
		exit(1);
	}
	return pqueue;
}

void *
top(struct Priority_Queue *pqueue)
{
	if(Count(pqueue)==0)
		return NULL;
	return pqueue->items[0];
}

void *
pop(struct Priority_Queue *pqueue)
{
	if(Count(pqueue) == 0)
		return NULL;
	void *rt = pqueue->items[0];
	swap(pqueue->items, 0, pqueue->count-1);
	--(pqueue->count);
	min_heapify(pqueue->items, pqueue->compare, pqueue->count, 0);
	return rt;
}

void 
push(struct Priority_Queue *pqueue, void *new_item)
{
	int cld, pt;
	++(pqueue->count);
	/* here maybe need double size */
	pqueue->items[pqueue->count-1] = new_item;
	cld = pqueue->count-1;
	pt = PARENT(cld);
	while(cld>0 && pqueue->compare(pqueue->items[cld], pqueue->items[pt])<0) {
		swap(pqueue->items, cld, pt);
		cld = pt;
		pt = PARENT(cld);
	}
}

void 
destroy(struct Priority_Queue *pqueue)
{
	free(pqueue->items);
	free(pqueue);
}
